#include "logic.hpp"
#include "config.hpp"

using namespace asio;
using namespace cv;
#define ISCORRECT(i,w,h) (abs(GET_CENTER(i).x-w/2)<40&&abs(GET_CENTER(i).y-h/2)<30)

Vision* major_Vision;
Vision* sub_Vision;
base_controller* major_Controller;

float xp,yp,xi,yi,xd,yd,xr,yr,wr,hr,xq,yq,wq,hq;
float lmp,lmi,lmd;
int yawOver = 1;
int xslow,//speed when aiming
	xmedium,//speed when colliding ball
	xfast;//speed when crossing
float yawthrott;//threshold when finding box
int choosePath = 0;//0 means choose right landmark if there were two
int qualifiedBall = 100,maxBall = 300;
int qualifiedFrame = 80;
PID_incre pidx(xp,xi,xd,300,-300);
PID_incre pidy(yp,yi,yd,250,-250);
PID_incre pidlm(lmp,lmi,lmd,300,-300);
SimpleKF KFx(320,xq,xr);
SimpleKF KFy(240,yq,yr);
SimpleKF KFw(80,wq,wr);
SimpleKF KFh(80,hq,hr);

int TARGETSIZE = 200;
int MINSIZETOCONTROL = 100;
int MAXSIZETOCONTROL = 400;
std::function<void()> func3 = [](){
	Stats pattern = MIDDLE_STATE;//行为状态;
	Veloc temp_vel={0};
	float yawRec = major_Controller->pose.yawmsg;
	const int yawOverTemp = yawOver;
	bool finishFinding = 0;
	while(1)
	{
		if(major_Controller->readyToUse()&&!major_Vision->undistort.empty())
		{
			if(major_Controller->droneStatus.sys_status!=MAV_STATE_STANDBY&&major_Controller->droneStatus.sys_status!=MAV_STATE_ACTIVE)
				major_Controller->arm_disarm(1);
			if(pattern!=TRACKING_LANDMARK)
				major_Vision->detectClassifier(1);//detect and draw all boxes on "Mat undistort"
			imshow("a",major_Vision->undistort);
			waitKey(1);
			bbox_t posTarget = major_Vision->targetFrame;
			posTarget.x =KFx.update(major_Vision->targetFrame.x);
			posTarget.y =KFy.update(major_Vision->targetFrame.y);
			posTarget.w =KFw.update(major_Vision->targetFrame.w);
			posTarget.h =KFh.update(major_Vision->targetFrame.h);
			
			
		}
	}
}